#include "stm32f10x.h"
#include "config.h"
#include "def.h"
#include "types.h"
#include "MultiWii.h"
#include "IMU.h"
#include "Sensors.h"

#include "math.h"
#include "timer.h"
#include "Ano_OF.h"
#include "DJI_Guidance_usat.h"
#include "ReferenceFrameSolver.h"

int16_t sonarAltMy; //平滑后的超声波高度
int16_t PressAltMy;//平滑后的气压计高度
uint8_t alt_switch = 0;//测距模块标志
int16_t alt_diff;//气压和超声波的差值

extern uint8_t sonar_out_of_date;// 超声波是否超出量程
void getEstimatedAttitude();
////////////////////////////


void computeIMU() {
	uint8_t axis;
	uint8_t t;
	static int16_t gyroADCprevious[3] = {0, 0, 0};
	static int16_t gyroADCinter[3];

	uint16_t timeInterleave = 0;
#if ACC
	ACC_getADC();

	getEstimatedAttitude();
#endif
#if GYRO
	Gyro_getADC();
#endif
	for (axis = 0; axis < 3; axis++)
		gyroADCinter[axis] = imu.gyroADC[axis];
	timeInterleave = micros();
	annexCode();//解析串口程序
	t = 0;
	while ((int16_t) (micros() - timeInterleave) < 650)
		t = 1; //empirical, interleaving delay between 2 consecutive reads
#ifdef LCD_TELEMETRY
	if (!t) annex650_overrun_count++;
#endif
#if GYRO
	Gyro_getADC();
#endif
	for (axis = 0; axis < 3; axis++) {
		gyroADCinter[axis] = imu.gyroADC[axis] + gyroADCinter[axis];
		// empirical, we take a weighted value of the current and the previous values
		imu.gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis]) / 3;
		gyroADCprevious[axis] = gyroADCinter[axis] >> 1;
		if (!ACC) imu.accADC[axis] = 0;
	}
#if defined(GYRO_SMOOTHING)
	static int16_t gyroSmooth[3] = {0,0,0};
	for (axis = 0; axis < 3; axis++) {
		imu.gyroData[axis] = (int16_t) ( ( (int32_t)((int32_t)gyroSmooth[axis] * (conf.Smoothing[axis]-1) )+imu.gyroData[axis]+1 ) / conf.Smoothing[axis]);
		gyroSmooth[axis] = imu.gyroData[axis];
	}
#elif defined(TRI)
	static int16_t gyroYawSmooth = 0;
	imu.gyroData[YAW] = (gyroYawSmroData[YAW])/3;
	gyroYawSmooth = imu.gyroData[YAW];
#endif
#if GYRO
	// capture frame solver
	//frame_offset_calculate();
#endif
}

// **************************************************
// Simplified IMU based on "Complementary Filter"
// Inspired by http://starlino.com/imu_guide.html
//
// adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198
//
// The following ideas was used in this project:
// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
// 3) C. Hastings approximation for atan2()
// 4) Optimization tricks: http://www.hackersdelight.org/
//
// Currently Magnetometer uses separate CF which is used only
// for heading approximation.
//
// **************************************************

//******  advanced users settings *******************
/* Set the Low Pass Filter factor for ACC
   Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time
   Comment this if  you do not want filter at all.
   unit = n power of 2 */
// this one is also used for ALT HOLD calculation, should not be changed
#ifndef ACC_LPF_FACTOR
#define ACC_LPF_FACTOR 4 // that means a LPF of 16
#endif

/* Set the Gyro Weight for Gyro/Acc complementary filter
   Increasing this value would reduce and delay Acc influence on the output of the filter*/
#ifndef GYR_CMPF_FACTOR
#define GYR_CMPF_FACTOR 10 //  that means a CMP_FACTOR of 1024 (2^10)
#endif

/* Set the Gyro Weight for Gyro/Magnetometer complementary filter
   Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
#define GYR_CMPFM_FACTOR 8 // that means a CMP_FACTOR of 256 (2^8)


typedef struct {
	int32_t X, Y, Z;
} t_int32_t_vector_def;

typedef struct {
	uint16_t XL;
	int16_t X;
	uint16_t YL;
	int16_t Y;
	uint16_t ZL;
	int16_t Z;
} t_int16_t_vector_def;

// note: we use implicit first 16 MSB bits 32 -> 16 cast. ie V32.X>>16 = V16.X
typedef union {
	int32_t A32[3];
	t_int32_t_vector_def V32;
	int16_t A16[6];
	t_int16_t_vector_def V16;
} t_int32_t_vector;

//return angle , unit: 1/10 degree
int16_t _atan2(int32_t y, int32_t x) {
	float z = y;
	int16_t a;
	uint8_t c;
	c = ABS(y) < ABS(x);
	if (c) {
		z = z / x;
	} else {
		z = x / z;
	}
	a = 2046.43 * (z / (3.5714 + z * z));
	if (c) {
		if (x < 0) {
			if (y < 0) a -= 1800;
			else a += 1800;
		}
	} else {
		a = 900 - a;
		if (y < 0) a -= 1800;
	}
	return a;
}

float InvSqrt(float x) {
	union {
		int32_t i;
		float f;
	} conv;
	conv.f = x;
	conv.i = 0x5f1ffff9 - (conv.i >> 1);
	return conv.f * (1.68191409f - 0.703952253f * x * conv.f * conv.f);
}

int32_t  __attribute__ ((noinline)) mul(int16_t a, int16_t b) {
	int32_t r;
	r = (int32_t) a * b;
	return r;
}

// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateV32(t_int32_t_vector *v, int16_t *delta) {
	int16_t X = v->V16.X;
	int16_t Y = v->V16.Y;
	int16_t Z = v->V16.Z;

	v->V32.Z -= mul(delta[ROLL], X) + mul(delta[PITCH], Y);
	v->V32.X += mul(delta[ROLL], Z) - mul(delta[YAW], Y);
	v->V32.Y += mul(delta[PITCH], Z) + mul(delta[YAW], X);
}

static int16_t accZ = 0;
unsigned long currentT_getEstimatedAttitude;
unsigned long previousT_getEstimatedAttitude;

// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void getEstimatedAttitude() {

	uint8_t axis;
	int32_t accMag = 0;
	float scale;
	int16_t deltaGyroAngle16[3];
	int32_t sqGX_sqGZ;
	static t_int32_t_vector EstG = {0, 0, (int32_t) ACC_1G << 16};
#if MAG
	static t_int32_t_vector EstM;
#else
	static t_int32_t_vector EstM = {0,(int32_t)1<<24,0};
#endif
	static uint32_t LPFAcc[3];
	float invG; // 1/|G|
	static int16_t accZoffset = 0;
	int32_t accZ_tmp = 0;
	currentT_getEstimatedAttitude = micros();
	// unit: radian per bit, scaled by 2^16 for further multiplication
	// with a delta time of 3000 us, and GYRO scale of most gyros, scale = a little bit less than 1
#define GYRO_SCALE (4 / 16.4 * PI / 180.0 / 1000000.0) //16.4 LSB = 1 deg/s
	scale = (currentT_getEstimatedAttitude - previousT_getEstimatedAttitude) * (GYRO_SCALE * 65536);
	previousT_getEstimatedAttitude = currentT_getEstimatedAttitude;
	// Initialization
	for (axis = 0; axis < 3; axis++) {
		// valid as long as LPF_FACTOR is less than 15
		imu.accSmooth[axis] = LPFAcc[axis] >> ACC_LPF_FACTOR;
		LPFAcc[axis] += imu.accADC[axis] - imu.accSmooth[axis];
		// used to calculate later the magnitude of acc vector
		accMag += mul(imu.accSmooth[axis], imu.accSmooth[axis]);
		// unit: radian scaled by 2^16
		// imu.gyroADC[axis] is 14 bit long, the scale factor ensure deltaGyroAngle16[axis] is still 14 bit long
		if (scale < 1)
			deltaGyroAngle16[axis] = imu.gyroADC[axis] * scale;
		else
			deltaGyroAngle16[axis] = imu.gyroADC[axis] * 0.988;
	}
	// we rotate the intermediate 32 bit vector with the radian vector (deltaGyroAngle16), scaled by 2^16
	// however, only the first 16 MSB of the 32 bit vector is used to compute the result
	// it is ok to use this approximation as the 16 LSB are used only for the complementary filter part
	rotateV32(&EstG, deltaGyroAngle16);
	rotateV32(&EstM, deltaGyroAngle16);
	// Apply complimentary filter (Gyro drift correction)
	// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
	// To do that, we just skip filter, as EstV already rotated by Gyro
	for (axis = 0; axis < 3; axis++) {
		if ((int16_t) (0.85 * ACC_1G * ACC_1G / 256) < (int16_t) (accMag >> 8) &&
			(int16_t) (accMag >> 8) < (int16_t) (1.15 * ACC_1G * ACC_1G / 256))
			EstG.A32[axis] += (int32_t) (imu.accSmooth[axis] - EstG.A16[2 * axis + 1]) << (16 - GYR_CMPF_FACTOR);
		accZ_tmp += mul(imu.accSmooth[axis], EstG.A16[2 * axis + 1]);
#if MAG
		EstM.A32[axis] += (int32_t) (imu.magADC[axis] - EstM.A16[2 * axis + 1]) << (16 - GYR_CMPFM_FACTOR);
#endif

	}


	if (EstG.V16.Z > ACCZ_25deg)
		f.SMALL_ANGLES_25 = 1;
	else
		f.SMALL_ANGLES_25 = 0;

	// Attitude of the estimated vector
	sqGX_sqGZ = mul(EstG.V16.X, EstG.V16.X) + mul(EstG.V16.Z, EstG.V16.Z);
	invG = InvSqrt(sqGX_sqGZ + mul(EstG.V16.Y, EstG.V16.Y));
	att.angle[ROLL] = _atan2(EstG.V16.X, EstG.V16.Z);
	att.angle[PITCH] = _atan2(EstG.V16.Y, InvSqrt(sqGX_sqGZ) * sqGX_sqGZ);

	//note on the second term: mathematically there is a risk of overflow (16*16*16=48 bits). assumed to be null with real values
	att.heading = _atan2(
			mul(EstM.V16.Z, EstG.V16.X) - mul(EstM.V16.X, EstG.V16.Z),
			(EstM.V16.Y * sqGX_sqGZ - (mul(EstM.V16.X, EstG.V16.X) + mul(EstM.V16.Z, EstG.V16.Z)) * EstG.V16.Y) * invG);
#if MAG
	att.heading += conf.mag_declination; // Set from GUI
#endif
	att.heading /= 10;

#if defined(THROTTLE_ANGLE_CORRECTION)
	cosZ = mul(EstG.V16.Z, 100) / ACC_1G;                                                    // cos(angleZ) * 100
	throttleAngleCorrection =
			THROTTLE_ANGLE_CORRECTION * constrain(100 - cosZ, 0, 100) >> 3;  // 16 bit ok: 200*150 = 30000
#endif

	// projection of ACC vector to global Z, with 1G subtructed
	// Math: accZ = A * G / |G| - 1G
	accZ = accZ_tmp * invG;
	if (!f.ARMED) {
		accZoffset -= accZoffset >> 3;
		accZoffset += accZ;
	}
	accZ -= accZoffset >> 3;
}

#define UPDATE_INTERVAL 25000    // 40hz update rate (20hz LPF on acc)
#define BARO_TAB_SIZE   21

#define ACC_Z_DEADBAND (ACC_1G>>5) // was 40 instead of 32 now


#define applyDeadband(value, deadband)  \
  if(ABS(value) < deadband) {           \
    value = 0;                          \
  } else if(value > 0){                 \
    value -= deadband;                  \
  } else if(value < 0){                 \
    value += deadband;                  \
  }

#if BARO
extern u16 Sonar_alt_u;
extern u8 flag_dis;
extern uint8_t isRCCVelChanged;
extern int16_t rccVelocity;
extern int32_t lastBaroAlt;

uint8_t getEstimatedAltitude() {
	int16_t error16;
	static int16_t baroVel = 0, lastBaroVel = 0, lastLastBaroVel = 0;
	int16_t zVelTarget = 0;
	static int16_t velError = 0, lastVelError = 0;
	static int16_t accDelta = 0, lastAccDelta = 0, lastLastAccDelta = 0;

	if (flag_dis == 3)
		flag_dis = 0;
	else
		return 0;
	alt.EstAlt = Sonar_alt_u;

	if (!f.BARO_MODE) return 0;
#if (defined(VARIOMETER) && (VARIOMETER != 2)) || !defined(SUPPRESS_BARO_ALTHOLD)
	//P

	error16 = constrain(AltHold - alt.EstAlt, -300, 300);

	if (isRCCVelChanged) {
		zVelTarget = rccVelocity;
	} else {
		zVelTarget = constrain(error16 * 2, -300, 300);
	}

	baroVel = mul(alt.EstAlt - lastBaroAlt, 1000000 / UPDATE_INTERVAL);
	lastBaroAlt = alt.EstAlt;
	baroVel = (baroVel + lastBaroVel + lastLastBaroVel) / 3;
	lastLastBaroVel = lastBaroVel;
	lastBaroVel = baroVel;
	baroVel = constrain(baroVel, -400, 400);
	velError = constrain(zVelTarget - baroVel, -400, 400);
	BaroPID = constrain((conf.pid[PIDALT].P8 * error16 >> 7), -150, +150);
	errorAltitudeI += conf.pid[PIDALT].I8 * velError >> 6;
	errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
	BaroPID += errorAltitudeI >> 9; //I in range +/-60
	accDelta = velError - lastVelError;
	accDelta = (accDelta + lastAccDelta + lastLastAccDelta) / 3;
	lastLastAccDelta = lastAccDelta;
	lastAccDelta = accDelta;
	lastVelError = velError;
	BaroPID += constrain(conf.pid[PIDALT].D8 * accDelta >> 5, -150, 150);
	debug[0] = BaroPID;
	debug[1] = error16;
#endif
	return 1;
}

#endif //BARO

uint16_t landDetectorStartTime;
uint16_t timeOnLand;
volatile bool landed = false;

void resetLandDetector() {
	landDetectorStartTime = millis();
	timeOnLand = 0;
	landed = false;
}

bool isGroundDetected() {
	return alt.vario < 10 && errorAltitudeI <= -900 && alt.EstAlt < 15;
}

void testLanding() {
	if (isGroundDetected()) {
		timeOnLand = millis() - landDetectorStartTime;
	} else {
		// we are still moving up or down
		resetLandDetector();
	}
	if (!landed && timeOnLand >= 200) {
		landed = true;
	}
}
